from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def create_nodes(context, *args, **kwargs):
    # 获取 vehicle_num 的运行时值
    vehicle_num = int(LaunchConfiguration('vehicle_num').perform(context))
    start_num = int(LaunchConfiguration('start_node').perform(context))
    end_num = int(LaunchConfiguration('end_node').perform(context))
    
    # 创建节点列表
    nodes = []
    for i in range(start_num,end_num):
        node = Node(
            package="uav_swarms",
            executable="communication_multirotor_node",
            name=f"communication_multirotor_node{i}",
            output="screen",
            emulate_tty=True,
            parameters=[
                {"uav_namespace": LaunchConfiguration('uav_namespace')},
                {"vehicle_ids": i},
                {"vehicle_num": vehicle_num},
                {"vehicle_type": LaunchConfiguration('vehicle_type')},
                {"default_height": LaunchConfiguration('default_height')},
                {"start_node": LaunchConfiguration('start_node')},
                {"end_node": LaunchConfiguration('end_node')},
            ]
        )
        nodes.append(node)
    return nodes

def generate_launch_description():
    # 声明参数
    vehicle_id_arg = DeclareLaunchArgument(
        'vehicle_ids',
        default_value="0",
        description='Vehicle ID.'
    )

    uav_namespace_arg = DeclareLaunchArgument(
        'uav_namespace',
        default_value='uav_swarms',
        description='Namespace for UAV swarms.'
    )

    vehicle_type_arg = DeclareLaunchArgument(
        'vehicle_type',
        default_value='drone',
        description='Type of vehicle.'
    )

    vehicle_num_arg = DeclareLaunchArgument(
        'vehicle_num',
        default_value="16",
        description='Number of vehicles.'
    )

    default_height_arg = DeclareLaunchArgument(
        'default_height',
        default_value="82.0",
        description='Default height for vehicles.'
    )

    start_node_arg = DeclareLaunchArgument(
        'start_node',
        default_value="0",
        description='Default height for vehicles.'
    )

    end_node_arg = DeclareLaunchArgument(
        'end_node',
        default_value="4",
        description='Default height for vehicles.'
    )

    # 使用 OpaqueFunction 动态创建节点
    dynamic_nodes = OpaqueFunction(function=create_nodes)

    # 返回 LaunchDescription
    return LaunchDescription([
        vehicle_id_arg,
        uav_namespace_arg,
        vehicle_type_arg,
        vehicle_num_arg,
        default_height_arg,
        start_node_arg,
        end_node_arg,
        dynamic_nodes,
    ])
